#!/usr/bin/env python
from robotparts import *
from vis import *
import numpy
import time

import cProfile
import pstats


#main loop
def main():

	#link0=link((0,0,0,0),'link0',None)
	#link1=link((100,90,0,90),'link1',link0,)
	#link2=link((100,0,0,45),'link2',link1,)
	#link3=link((100,0,0,45),'link3',link2,)

	#links=[link0,link1,link2,link3]

	
	#create links...
	links=[]
	d=-500
	i=0
	
	parent=None
	alpha=0.0
	theta=0.0
	for x in range(6):
		theta+=60
		links.append(link((100,alpha,0,theta),"link"+str(x),parent,(10,1,1,1),False))

	
	
	links.append(link((50,90,0,0),"link"+str(x),links[0]))
	links.append(link((100,0.0,0,-30),"link"+str(x),links[-1]))
	links.append(link((100,0.0,0,90),"link"+str(x),links[-1]))

	links.append(link((50,90,0,0),"link"+str(x),links[1]))
	links.append(link((100,0.0,0,-30),"link"+str(x),links[-1]))
	links.append(link((100,0.0,0,90),"link"+str(x),links[-1]))

	links.append(link((50,90,0,0),"link"+str(x),links[2]))
	links.append(link((100,0.0,0,-30),"link"+str(x),links[-1]))
	links.append(link((100,0.0,0,90),"link"+str(x),links[-1]))

	links.append(link((50,90,0,0),"link"+str(x),links[3]))
	links.append(link((100,0.0,0,-30),"link"+str(x),links[-1]))
	links.append(link((100,0.0,0,90),"link"+str(x),links[-1]))

	links.append(link((50,90,0,0),"link"+str(x),links[4]))
	links.append(link((100,0.0,0,-30),"link"+str(x),links[-1]))
	links.append(link((100,0.0,0,90),"link"+str(x),links[-1]))

	links.append(link((50,90,0,0),"link"+str(x),links[5]))
	links.append(link((100,0.0,0,-30),"link"+str(x),links[-1]))
	links.append(link((100,0.0,0,90),"link"+str(x),links[-1]))
		

	visualizer = VisThread(links,(1024,768))
	visualizer.start()

	eng=engine(links)
	eng.start()

	while visualizer.running:
		time.sleep(0.01)
		t=time.time()
		
		for x in [7,10,13,16,19,22]:
			links[x].theta=20*sin(t*0.4*2*pi)

		eng.UpdateEvent.set()

	eng.running=False

	




#run main function
#if __name__ == '__main__': main()
cProfile.run('main()', 'mainprof')
p = pstats.Stats('mainprof')
p.sort_stats('cumulative').print_stats(10)



